

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "platform.h"

#include "build/atomic.h"
#include "build/build_config.h"
#include "build/debug.h"

#include "common/maths.h"
#include "common/utils.h"

#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"

#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
#include "drivers/accgyro/accgyro_mpu.h"


//mpuResetFnPtr mpuResetFn;

#define MPU_INQUIRY_MASK   0x7E

static bool mpuReadRegisterI2C(uint8_t addr, uint8_t reg, uint8_t* data, uint8_t length)
{
    //UNUSED(bus);
    const bool ack = i2cRead(I2C_DEVICE, addr, reg, length, data);
    return ack;
}

bool mpuWriteRegisterI2C(uint8_t addr, uint8_t reg, uint8_t data)
{
    //UNUSED(bus);
    const bool ack = i2cWrite(I2C_DEVICE, addr, reg, data);
    return ack;
}

static mpu6050Resolution_e mpu6050FindRevision(gyroDev_t *gyro, uint8_t addr)
{
    bool ack;
    UNUSED(ack);

    uint8_t readBuffer[6];
    uint8_t revision;
    uint8_t productId;

    // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
    // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c

    // determine product ID and accel revision
    ack = mpuReadRegisterI2C(addr, MPU_RA_XA_OFFS_H, readBuffer, 6);
    revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
    if (revision) {
        /* Congrats, these parts are better. */
        if (revision == 1) {
            gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
        } else if (revision == 2) {
            gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
        } else if ((revision == 3) || (revision == 7)) {
            gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
        } else {
            failureMode(FAILURE_ACC_INCOMPATIBLE);
        }
    } else {
        ack = mpuReadRegisterI2C(addr, MPU_RA_PRODUCT_ID, &productId, 1);
        revision = productId & 0x0F;
        if (!revision) {
            failureMode(FAILURE_ACC_INCOMPATIBLE);
        } else if (revision == 4) {
            gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
        } else {
            gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
        }
    }
    return gyro->mpuDetectionResult.resolution;
}

bool mpuAccRead(accDev_t *acc)
{
    uint8_t data[6];

    const bool ack1 = mpuReadRegisterI2C(MPU6050_ADDRESS_A, MPU_RA_ACCEL_XOUT_H, data, 6);
    if (!ack1) {
        return false;
    }

    int32_t x0 = (int16_t)((data[0] << 8) | data[1]);
    int32_t y0 = (int16_t)((data[2] << 8) | data[3]);
    int32_t z0 = (int16_t)((data[4] << 8) | data[5]);

#ifdef DUAL_MPU
    const bool ack2 = mpuReadRegisterI2C(MPU6050_ADDRESS_B, MPU_RA_ACCEL_XOUT_H, data, 6);
    if (!ack2) {
        return false;
    }

    int32_t x1 = (int16_t)((data[0] << 8) | data[1]);
    int32_t y1 = (int16_t)((data[2] << 8) | data[3]);
    int32_t z1 = (int16_t)((data[4] << 8) | data[5]);

    acc->ADCRaw[X] = (int16_t)((x0+x1)>>1);
    acc->ADCRaw[Y] = (int16_t)((y0+y1)>>1);
    acc->ADCRaw[Z] = (int16_t)((z0+z1)>>1);
#else

    acc->ADCRaw[X] = (int16_t)(x0);
    acc->ADCRaw[Y] = (int16_t)(y0);
    acc->ADCRaw[Z] = (int16_t)(z0);

#endif
    return true;
}

bool mpuGyroRead(gyroDev_t *gyro)
{
    uint8_t data[6];

    const bool ack1 = mpuReadRegisterI2C(MPU6050_ADDRESS_A, MPU_RA_GYRO_XOUT_H, data, 6);
    if (!ack1) {
        return false;
    }

    int32_t x0 = (int16_t)((data[0] << 8) | data[1]);
    int32_t y0 = (int16_t)((data[2] << 8) | data[3]);
    int32_t z0 = (int16_t)((data[4] << 8) | data[5]);

#ifdef DUAL_MPU
    const bool ack2 = mpuReadRegisterI2C(MPU6050_ADDRESS_B, MPU_RA_GYRO_XOUT_H, data, 6);
    if (!ack2) {
        return false;
    }

    int32_t x1 = (int16_t)((data[0] << 8) | data[1]);
    int32_t y1 = (int16_t)((data[2] << 8) | data[3]);
    int32_t z1 = (int16_t)((data[4] << 8) | data[5]);

    gyro->gyroADCRaw[X] = (int16_t)((x0+x1)>>1);
    gyro->gyroADCRaw[Y] = (int16_t)((y0+y1)>>1);
    gyro->gyroADCRaw[Z] = (int16_t)((z0+z1)>>1);
#else

    gyro->gyroADCRaw[X] = (int16_t)(x0);
    gyro->gyroADCRaw[Y] = (int16_t)(y0);
    gyro->gyroADCRaw[Z] = (int16_t)(z0);

#endif
    return true;
}

gyroOverflow_e mpuGyroCheckOverflow(const gyroDev_t *gyro)
{
    // we cannot detect overflow directly, so assume overflow if absolute gyro rate is large
    gyroOverflow_e ret = GYRO_OVERFLOW_NONE;
    const int16_t overflowValue = 0x7C00; // this is a slightly conservative value, could probably be as high as 0x7FF0
    if (gyro->gyroADCRaw[X] > overflowValue || gyro->gyroADCRaw[X] < -overflowValue) {
        ret |= GYRO_OVERFLOW_X;
    }
    if (gyro->gyroADCRaw[Y] > overflowValue || gyro->gyroADCRaw[Y] < -overflowValue) {
        ret |= GYRO_OVERFLOW_Y;
    }
    if (gyro->gyroADCRaw[Z] > overflowValue || gyro->gyroADCRaw[Z] < -overflowValue) {
        ret |= GYRO_OVERFLOW_Z;
    }
    return ret;
}

#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
    UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI

    uint8_t sensor = MPU_NONE;
    UNUSED(sensor);

    return false;
}
#endif

void mpuDetect(gyroDev_t *gyro, uint8_t addr, int32_t* info)
{
    // MPU datasheet specifies 30ms.
    delay(35);

    uint8_t sig = 0;
#ifdef USE_I2C
    //gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE;
    //gyro->bus.busdev_u.i2c.address = MPU_ADDRESS;
    bool ack = mpuReadRegisterI2C(addr, MPU_RA_WHO_AM_I, &sig, 1);
#else
    bool ack = false;
#endif
    if (ack) {
        //gyro->mpuConfiguration.readRegFn = mpuReadRegisterI2C;
    } else {
#ifdef USE_SPI
        detectSPISensorsAndUpdateDetectionResult(gyro);
#endif
        return;
    }

    // If an MPU3050 is connected sig will contain 0.
    uint8_t inquiryResult;
    ack = mpuReadRegisterI2C(addr, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
    inquiryResult &= MPU_INQUIRY_MASK;
    if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
        gyro->mpuDetectionResult.sensor = MPU_3050;
        return;
    }

    sig &= MPU_INQUIRY_MASK;
    if (sig == MPUx0x0_WHO_AM_I_CONST) {
        gyro->mpuDetectionResult.sensor = MPU_60x0;
        mpu6050Resolution_e resolution = mpu6050FindRevision(gyro, addr);
        info[0] = 1;
        info[1] = resolution;

    } else if (sig == MPU6500_WHO_AM_I_CONST) {
        gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
    }
}
